/**
 * Deng's FOC Simple FOC Studio 电机PID调参数程序 测试库：SimpleFOC 2.1.1 测试硬件：灯哥开源FOC V1.0
 * 使用前请首先按照首页和文档所示配置 SimpleFOC Studio
 * 在使用自己的电机时，请一定记得修改默认极对数，即 BLDCMotor(7) 中的值，设置为自己的极对数数字
 * 程序默认设置的供电电压为 7.4V,用其他电压供电请记得修改 voltage_power_supply , voltage_limit 变量中的值
 * 默认PID针对的电机是 Ipower 4008 ，使用自己的电机需要修改PID参数，才能实现更好效果
 */

#include <SimpleFOC.h>

// 设置一号无刷电机 Motor instance
// BLDC motor & driver instance
//那个括号里面的14是设置极对数的，需要查询电机的参数进行设置
BLDCMotor motor = BLDCMotor(14);
BLDCDriver3PWM driver = BLDCDriver3PWM(32, 33, 25, 22);
// 设置二号无刷电机
BLDCMotor motor1 = BLDCMotor(14);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(26, 27, 14, 12);

// encoder instance
Encoder encoder = Encoder(5, 18, 4096); //这个只能有三个参数，跟以前不一样了
Encoder encoder1 = Encoder(23, 19, 4096);

//命令设置
Commander command = Commander(Serial);
void doMotor(char *cmd) { command.motor(&motor, cmd); }

// Interrupt routine intialisation
// channel A and B callbacks
// 1号编码器
void doA() { encoder.handleA(); }
void doB() { encoder.handleB(); }
// 2号编码器
void doA1() { encoder1.handleA(); }
void doB1() { encoder1.handleB(); }

void setup()
{
	encoder.init();
	encoder.enableInterrupts(doA, doB);
	encoder1.init();
	encoder1.enableInterrupts(doA1, doB1);
	//连接motor对象与传感器对象
	motor.linkSensor(&encoder);
	motor1.linkSensor(&encoder1);

	//供电电压设置 [V]
	driver.voltage_power_supply = 12;
	driver.init();
	driver1.voltage_power_supply = 12;
	driver1.init();

	//连接电机和driver对象
	motor.linkDriver(&driver);
	motor1.linkDriver(&driver1);
	// FOC模型选择
	motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
	//运动控制模式设置
	motor.controller = MotionControlType::velocity;
	//速度PI环设置
	motor.PID_velocity.P = 0.2;
	motor.PID_velocity.I = 20;
	//角度P环设置
	motor.P_angle.P = 20;
	//最大电机限制电机
	motor.voltage_limit = 12;

	//速度低通滤波时间常数
	motor.LPF_velocity.Tf = 0.01;
	//设置最大速度限制
	motor.velocity_limit = 40;

	Serial.begin(115200); // the higher the better

	//初始化电机
	motor.init();
	//初始化 FOC
	motor.initFOC();
	command.add('M', doMotor, "my motor");
	// tell the motor to use the monitoring
	motor.useMonitoring(Serial);
	//  motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE;
	motor.monitor_downsample = 10; // disable monitor at first - optional
}
void loop()
{
	motor.loopFOC();
	motor.monitor();
	motor.move();

	command.run();
}
